/**
 *             _..--""-.                  .-""--.._
           _.-'         \ __...----...__ /         '-._
         .'      .:::...,'              ',...:::.      '.
        (     .'``'''::;                  ;::'''``'.     )
         \             '-)              (-'             /
          \             /                \             /
           \          .'.-.            .-.'.          /
            \         | \0|            |0/ |         /
            |          \  |   .-==-.   |  /          |
             \          `/`;          ;`\`          /
              '.._      (_ |  .-==-.  | _)      _..'
                  `"`"-`/ `/'        '\` \`-"`"`
                       / /`;   .==.   ;`\ \
                 .---./_/   \  .==.  /   \ \
                / '.    `-.__)       |    `"
               | =(`-.        '==.   ;
                \  '. `-.           /
                 \_:_)   `"--.....-'

                 file: motor.cpp
                 description: Motor hardware.
                 authors: Michiel and Peter
 */

#include "gripper/gripper.h"

/**
 * Init motor in the torque mode.
 * Note: We used the dpr2 example for this.
 */
void Gripper::init()
{
	ROS_INFO("Initialize gripper.");

	// read parameters (like the dpr2 robot)
	std::string motor_port_name, motor_config_name;

	ROS_ASSERT(nh_.getParam("motor_port", motor_port_name));
	ROS_ASSERT(nh_.getParam("motor_config", motor_config_name));

	// load motor configuration
	CXMLConfiguration motor_config_xml;
	ROS_ASSERT(motor_config_xml.loadFile(motor_config_name));

	CDxlConfig config;
	config.readConfig(motor_config_xml.root().section("motor"));
	motor_ = new C3mxlROS(motor_port_name.c_str());
	motor_->setConfig(&config);

	// Initialize motor
	ros::Rate init_rate(1);
	while (ros::ok() && motor_->init() != DXL_SUCCESS)
	{
		ROS_WARN_ONCE("Couldn't initialize gripper motor, will continue trying every second");
		init_rate.sleep();
	}
}

/**
 * Return motor interface.
 * @return	C3mxlROS*	motor interface.
 */
C3mxlROS *Gripper::getInterfaceMotor()
{
	return motor_;
}
